#!/usr/bin/env python

# --------
# Imports:
# --------
from math import acos, exp, sqrt, pi

# --------------------
def quat2heading(quat):
# --------------------
	x,y,z,w = quat if type(quat) == tuple else (quat.x, quat.y, quat.z, quat.w)

	sgnZ = 1 if z >= 0 else -1
	return 2*acos(w)*sgnZ 

# ------------------
def clipAngle(theta):
# ------------------
	# Clip the angle theta (in radians) to be between -PI and +PI
	while theta > 3.14159265:
		theta -= 6.28318531
	while theta < -3.14159265:
		theta += 6.28318531
	return theta 

# ----------------------------
def gaussian(mu, sigma, value):
# ----------------------------
	# Return the probability of getting value from a normal distribution with mu and sigma
	val = -0.5*(((value-mu)*(value-mu))/(sigma*sigma))
	mult = 1.0/(sqrt(2*pi*sigma*sigma))
	return mult*exp(val)

